#include "aStar.h"
#include <vector>

/*****************************************************************/
/******************************Anode******************************/
/*****************************************************************/

Anode::Anode()
{
	node = NULL;
	Fvalue = 0;
	Gvalue = 0;
	Hvalue = 0;
	parent = NULL;
}

Anode::Anode(GridNode* n, int f, int g, int h, Anode* p)
{
	node = n;
	Fvalue = f;
	Gvalue = g;
	Hvalue = h;
	parent = p;
}

void Anode::setFvalue(int f)	// set the A* values
{
	Fvalue = f;
}

void Anode::setGvalue(int g)
{
	Gvalue = g;
}

void Anode::setHvalue(int h)
{
	Hvalue = h;
}

int Anode::getFvalue()		// get the A* values
{
	return Fvalue;
}

int Anode::getGvalue()
{
	return Gvalue;
}

int Anode::getHvalue()
{
	return Hvalue;
}

/*****************************************************************/
/******************************Astar******************************/
/*****************************************************************/

Astar::Astar()
{
	grid = NULL;
	start = NULL;
	goal = NULL;
}

Astar::~Astar()							// delete any memory allocated
{
	//std::list<Anode*>::iterator it;
	//for(it = openList.begin(); it != openList.end(); it++)
	//	delete(*it);
	//for(it = closedList.begin(); it != closedList.end(); it++)
	//	if( *it != NULL )
	//		delete(*it);
}

Astar::Astar(Grid* g, GridNode* startNode, GridNode* goalNode)
{
	grid = g;
	start = startNode;
	goal = goalNode;
}

void Astar::setGrid(Grid* g)				// set the grid to use
{
	grid = g;
}

void Astar::setStartNode(GridNode* node)	// set starting node
{
	start = node;
}

void Astar::setGoalNode(GridNode* node)		// set goal node
{
	goal = node;
}

//Helper method for addNodesToOpen. Need to choose which nodes initially go in the openList.
//Also need to figure out the f, g, h values to create the Anode in which to store the node we are considering.
// isN = to check if the consider node is NORTH of the parent.  The boolean values can have two active at a time.
// if isN == true and isE == true, then consider is northeast of the parent.
void Astar::decide(GridNode* consider, Anode* parent, bool isN, bool isE, bool isS, bool isW)
{
	if( consider == NULL ) //GridNode does not exist, or is out of bounds (these are probably the same thing :-p)
		return;
	if( !(consider->isClear()) ) //Don't do anything, consider doesn't belong on any list.
		return;

	GridNode* north = grid->getNorthNode(consider);
	GridNode* east = grid->getEastNode(consider);
	GridNode* south = grid->getSouthNode(consider);
	GridNode* west = grid->getWestNode(consider);
	if( isN && isE ) //Special condition check:  Cannot cut corners, will clip through obstacles.
	{
		if( !(south == NULL) && !(south->isClear()) )
			return;
		if( !(west == NULL) && !(west->isClear()) )
			return;
	}
	else if( isN && isW )
	{
		if( !(south == NULL) && !(south->isClear()) )
			return;
		if( !(east == NULL) && !(east->isClear()) )
			return;
	}
	else if( isS && isE )
	{
		if( !(north == NULL) && !(north->isClear()) )
			return;
		if( !(west == NULL) && !(west->isClear()) )
			return;
	}
	else if( isS && isW )
	{
		if( !(north == NULL) && !(north->isClear()) )
			return;
		if( !(east == NULL) && !(east->isClear()) )
			return;
	} //END SPECIAL CONDITION CHECK

	std::list<Anode*>::iterator it; //Check if there's any Anode that contains the GridNode we're considering
	for( it = openList.begin(); it != openList.end(); it++ )
	{
		Anode* aNode = *it;
		if( aNode->getGridNode()->equals(consider) )//consider is already on the open list, check to make sure the values aren't less from this parent than from the previous parent.
		{
			int tempGvalue = parent->getGvalue() + grid->getGcost( parent->getGridNode(), consider );
			if( tempGvalue < aNode->getGvalue() )
			{
				aNode->setGvalue(tempGvalue);
				aNode->setFvalue( tempGvalue + aNode->getHvalue() );
				aNode->setParent( parent );
			}

			return;
		}
	}
	
	for( it = closedList.begin(); it != closedList.end(); it++ )
	{
		Anode* aNode = *it;
		if( aNode->getGridNode()->equals(consider) )// consider has already been processed, don't go any further
			return;
	}

	int g = grid->getGcost( consider, parent->getGridNode() ) + parent->getGvalue();
	int h = grid->getDistance(consider, goal);					
	int f = g + h;
	openList.push_back( new Anode(consider, f, g, h, parent) ); //Damn memory leak :(
}

void Astar::addNodesToOpen(Anode* node)		// add this node's neighbors to the open list if they are walkable and not already there.  Make parent point to currentNode
{
	decide( grid->getNorthNode(node->getGridNode()), node, true );
	decide( grid->getNENode(node->getGridNode()), node, true, true );
	decide( grid->getEastNode(node->getGridNode()), node, false, true );
	decide( grid->getSENode(node->getGridNode()), node, false, true, true );
	decide( grid->getSouthNode(node->getGridNode()), node, false, false, true );
	decide( grid->getSWNode(node->getGridNode()), node, false, false, true, true );
	decide( grid->getWestNode(node->getGridNode()), node, false, false, false, true );
	decide( grid->getNWNode(node->getGridNode()), node, true, false, false, true );
	openList.sort();
}

void Astar::moveFromOpenToClosed(Anode* node)	// remove the node from the open list and put it on the closed list
{
	openList.remove(node);
	closedList.push_back(node);
}

Anode* Astar::getNextNode()					// get the next node that should be considered
{
	return openList.front();
	//std::list<Anode*>::iterator it = openList.begin();
	//int lowestF = (*it)->getFvalue();
	//Anode* theOne = *it;
	//for( unsigned int i = 0; i < openList.size(); i++, ++it )
	//{
	//	int value = (*it)->getFvalue();
	//	if( value < lowestF )
	//	{
	//		lowestF = value;
	//		theOne = *it;
	//	}
	//}

	//return theOne;
}

bool Astar::findPath()						// run the A* algorithm on the grid and store the result in path
{
	Anode currentPos(start);
	Anode* current = &currentPos;
	openList.push_back( current );
	moveFromOpenToClosed( current );
	while( !current->getGridNode()->equals(goal) )				 //Start the algorithm;  go until we reach the goal node.
	{
		addNodesToOpen( current );
		if( openList.empty() ) //We could not find a path.
			return false;

		current = getNextNode();
		moveFromOpenToClosed( current );
	}
	
	Anode* tempCurrent = current;
	int i;
	for( i = 0; (tempCurrent = tempCurrent->getParent()) != NULL; i++ ); //This sucks, but I think it's the only way to count how many nodes are in the path
	while( i-- != 0 )
	{
		GridNode* currentGN = current->getGridNode();

		if( currentGN->equals(goal) )
			currentGN->contains = 'G';
		else
			currentGN->contains = '0' + i%10;

		path.push_front( currentGN );
		current = current->getParent();
	}

	start->contains = 'S';
	return true;
}

std::list<GridNode*>* Astar::getPath()			// get the path node by node starting at 0
{
	return &path;
}